#include"Predictor.h"
#define SHOWPRE
//#define SHOWCOM
void Predictor::predictor(float _xerr,float _yerr){
    xerr=_xerr;
    yerr=_yerr;
#ifdef SHOWPRE
    cout<<"pre:  "<<xerr_predicted<<"  "<<yerr_predicted<<endl;         
    cout<<"real: "<<xerr<<"  "<<yerr<<endl;
#endif
    dt = (float)(getTickCount()* 1000 / getTickFrequency()) - t ;
    vx=(xerr-lastx)/dt;
    vy=(yerr-lasty)/dt;
    ax=(vx-lastvx)/dt;
    ay=(vy-lastvy)/dt;
    xerr_predicted=float(xerr+(vx*dt+ax*dt*dt/2));
    yerr_predicted=float(yerr+(vy*dt+ay*dt*dt/2));
    lastx=xerr;
    lasty=yerr;
    lastvx=vx;
    lastvy=vy;
#ifdef SHOWCOM	
    showcom.setTo(Scalar(255,255,255,0));
    circle(showcom,Point(xerr*10+500,yerr*10+500),5,Scalar(0,0,250),3,8);
    circle(showcom,Point(xerr_predicted*10+500,yerr_predicted*10+500),10,Scalar(0,250,0),3,8);
    //cout<<"dt:"<<dt<<" vx:"<<vx<<" ax:"<<ax<<endl;
#endif
     t=(float)getTickCount()* 1000 / getTickFrequency();
    return ;
}
float Predictor::get_pitch(){
    return yerr_predicted;
}
float Predictor::get_yaw(){
    return xerr_predicted;
}